#pragma once

#include<rclcpp/rclcpp.hpp>
#include<iostream>
#include<fstream>
#include<string>
#include<string.h>
#include<time.h>
#include<stdio.h>
#include<sys/time.h>
#include<memory>

#include<math.h>
#include <Eigen/Dense>

#include"base_interface/msg/hit.hpp"
#include"base_interface/msg/cv_data.hpp"
#include"base_interface/msg/mcu_comm.hpp"


#include "hit_ctrl/qlearning.h"
#include <time.h>



#define PI         3.1415926

#define g          9.81f
#define target_h   0.5             //v2=2gh   h设为0.5m

using namespace std;

//-------test---------------------
using namespace std::chrono_literals;
//-------------------------


//-------------qlearning 的相关变量---------------
 float     ballVelWish;    
 float    anglevelWish;
 float         q_value;
//-------------------------------------------


Eigen::Vector2f Throw_the_ball_oneself(float cv_end_pitch,float cv_end_yaw);
void The_position_of_the_batting_platform();

class HitCtrl : public rclcpp::Node
{
public:
    HitCtrl();
    ~HitCtrl(){};
 void CvMessageCallback(const base_interface::msg::CvData::SharedPtr cv_msg);
 void McuCommCallback(const base_interface::msg::McuComm::SharedPtr mcu_msg);

public:
    rclcpp::Publisher<base_interface::msg::Hit>::SharedPtr hit_pub;
    rclcpp::Subscription<base_interface::msg::CvData>::SharedPtr cvdata_receive;

    rclcpp::Subscription<base_interface::msg::McuComm>::SharedPtr mcu_receive;
     
    rclcpp::Time base_time;

    //----------test-------------------------------------
         rclcpp::TimerBase::SharedPtr timer_;
    //-------------------------------------------
};

struct 
{
  /* data */
  float pitch;                             //平台俯仰——弧度制
  float yaw;                               //平台偏航——弧度制
  float roll;                              //平台横滚——弧度制
  float vel;                               //平台颠球速度         
  float p3_0_x;                            //平台中心相对于原点的坐标
  float p3_0_y;                            //平台中心相对于原点的坐标
  float p3_0_z;                            //平台中心相对于原点的坐标
  float ball_out_policy_theta;             //球离开时与击球平面之间的期望夹角
  float ball_out_policy_vel = 176.49;     //球离开击球平面时的期望出射速度
  float ball_out_policy_pitch = 28.79/180*PI;    //球离开击球平面时相对于地面的期望俯仰
  float ball_out_policy_yaw = 75.87/180*PI;               //球离开击球平面时相对于地面的期望偏航
  float hit_z;                             //平台击球平面
  float hit_time;                          //平台击球时间
}policy_ball;                              //上层机构的决策



struct 
{
  /* data */
  float pitch;                   //球的俯仰——弧度制
  float yaw ;                     //球的偏航——弧度制
  float roll;                                   //球的横滚——弧度制——没有横滚
  float p3_0_x;                                 //球相对于原点的坐标——视觉给出
  float p3_0_y;                                 //球相对于原点的坐标——视觉给出
  float p3_0_z;                                 //球相对于原点的坐标——视觉给出
  float come_state_gama;                        //球来时与击球平台法线之间的实际夹角——根据视觉给的俯仰和偏航速度计算出来
  float come_state_vel ;                //球来时相对于平台的入射实际速度——视觉给出
  float out_state_gama;                        //球离开时与击球平面之间的实际夹角——根据视觉给的俯仰和偏航速度计算出来
  float out_state_vel ;                          //球离开时相对于平台的实际出射速度——根据视觉直接得出
  float out_state_pitch = 31.5/180*PI;
  float out_state_yaw = 72.67/180*PI; 
}state_ball;                                    //球的状态











